// Bsp_pcontrol.c
#include "Bsp_Uart.h"
#include "Bsp_pcontrol.h"
#include <stdio.h>
#include "Bsp_motion.h"
#include "Bsp_OLED.h"


// 定义帧头和帧尾的标志
#define FRAME_HEADER 0x55
#define FRAME_TAIL 0xBB

// 简化状态枚举，用于解析接收到的数据帧
typedef enum {
    IDLE,       // 空闲状态，等待帧头
    HEADER,    // 等待第一个帧头
    COMMAND,    // 命令状态，接收命令码
    LENGTH,     // 长度状态，接收数据长度
    DATA,       // 数据状态，接收数据内容
    CHECKSUM,   // 校验和状态，验证数据完整性
    TAIL        // 帧尾状态，确认帧尾是否正确
} FrameState;

// 当前解析状态
static FrameState state = IDLE;
// 接收的命令码
static uint8_t command = 0;
// 接收的数据长度
static uint8_t length = 0;
// 数据缓冲区
static uint8_t dataBuffer[256];
// 数据索引
static uint8_t dataIndex = 0;
// 校验和
static uint8_t checksum = 0;

/**
 * @brief 处理接收到的UART数据
 * @details 从UART缓冲区读取数据并逐字节解析
 */
void Process_UART_Data(void) {
    uint8_t data;
    while (!UART_Buffer_IsEmpty()) { // 检查UART缓冲区是否为空
        UART_Buffer_Get(&data);      // 获取一个字节的数据
        Handle_UART_Byte(data);      // 调用字节处理函数
//      printf("data = 0x%02X",data);

    }
    checksum = 0;
}

/**
 * @brief 处理单个字节数据
 * @param data 接收到的字节数据
 * @details 根据当前状态解析数据帧
 */
void Handle_UART_Byte(uint8_t data) {
    switch (state) {
        case IDLE: // 空闲状态
            if (data == FRAME_HEADER) { // 检查是否为帧头
                state = HEADER;         // 进入帧头状态
            }
            break;
        case HEADER: // 第二个帧头
            if (data == FRAME_HEADER) {
                state = COMMAND; // 等待第二个帧头
            } else {
                state = IDLE; // 帧头错误，返回空闲状态
            }
            break;
        case COMMAND: // 命令状态
            command = data;             // 保存命令码
            checksum += data;           // 更新校验和
            state = LENGTH;             // 进入长度状态
            break;
        case LENGTH: // 长度状态
            length = data;              // 保存数据长度
            checksum += data;           // 更新校验和
            dataIndex = 0;              // 初始化数据索引
            if (length > 0) {           // 如果有数据，进入数据状态
                state = DATA;
            } else {                    // 否则直接进入校验和状态
                state = CHECKSUM;
            }
            break;
        case DATA: // 数据状态
            if (dataIndex < length) {   // 检查数据索引是否在范围内
                dataBuffer[dataIndex++] = data; // 保存数据
                checksum += data;       // 更新校验和
            }
            if (dataIndex == length) {  // 数据接收完成，进入校验和状态
                state = CHECKSUM;
            }
            break;
        case CHECKSUM: // 校验和状态
        	printf("CHECKSUM:0x%02X\n",checksum);
            if (checksum == data) {     // 检查校验和是否匹配
                state = TAIL;           // 进入帧尾状态
                OLED_ShowString(4, 1, "Check success!\n");
            } else {
                printf("Checksum error!\n"); // 校验和错误
                OLED_ShowString(4, 1, "Check error!\n");
                state = IDLE;           // 返回空闲状态
            }
            break;
        case TAIL: // 帧尾状态
            if (data == FRAME_TAIL) {   // 检查帧尾是否正确
                Handle_Data_Frame(dataBuffer, length); // 处理数据帧
            } else {
                printf("Tail error!\n"); // 帧尾错误
            }
            checksum = 0; 				 // 重置校验和
            state = IDLE;               // 返回空闲状态
            break;
        default:                        // 默认情况
            state = IDLE;               // 返回空闲状态
            break;
    }
}

/**
 * @brief 处理接收到的数据帧
 * @param data 数据缓冲区
 * @param length 数据长度
 * @details 根据命令码处理不同的控制命令
 */
void Handle_Data_Frame(uint8_t *data, uint8_t length) {
    printf("Received frame: Command=0x%02X, Length=%d\n", command, length); // 打印接收到的命令和长度
    for (int i = 0; i < length; i++) {                                      // 打印数据内容
        printf("Data[%d]: 0x%02X\n", i, data[i]);
    }
    // 根据命令码执行不同操作
    switch (command) {

        case 0x01: // 命令码 0x01：LED 控制
            
            // 处理 LED 控制逻辑
            break;

        case 0x02: // 命令码 0x02：小车速度控制
            if (length == 6) {
            int16_t vx = (data[0] << 8) | data[1];
            int16_t vy = (data[2] << 8) | data[3];
            int16_t vz = (data[4] << 8) | data[5];
            printf("Received Speed: Vx=%d, Vy=%d, Vz=%d\n", vx, vy, vz);
            Motion_Ctrl(vx, vy, vz);
            // 调用实际控制函数...
            } 
            else {
                printf("Data length error for command 0x02, expected 6 bytes, got %d\n", length);
            }
            // 处理小车速度控制逻辑
            break;

            
        case 0x03: // 命令码 0x03：蜂鸣器控制
            // 处理蜂鸣器控制逻辑
            break;
        default:   // 未知命令
            printf("Unknown command: 0x%02X\n", command);// 打印未知命令
            break;
    }
}
